17. Landmarks Quiz Solution
Observations in the car coordinate system can be transformed into map coordinates ( \text{x}_m and \text{y}_m ) by passing car observation coordinates ( \text{x}_c and \text{y}_c ), map particle coordinates ( \text{x}_p and \text{y}_p ), and our rotation angle (-90 degrees) through a homogenous transformation matrix. This homogenous transformation matrix, shown below, performs rotation and translation.
Homogenous Transformation
Matrix multiplication results in:
\text{x}_m= \text{x}_p + (\cos\theta \times \text{x}_c) - (\sin\theta \times \text{y}_c)
\text{y}_m= \text{y}_p + (\sin\theta \times \text{x}_c) + (\cos\theta \times \text{y}_c)
Quiz Solutions
Observation 1 Solution
#include <cmath>
#include <iostream>
int main() {
// define coordinates and theta
double x_part, y_part, x_obs, y_obs, theta;
x_part = 4;
y_part = 5;
x_obs = 2;
y_obs = 2;
theta = -M_PI/2; // -90 degrees
// transform to map x coordinate
double x_map;
x_map = x_part + (cos(theta) * x_obs) - (sin(theta) * y_obs);
// transform to map y coordinate
double y_map;
y_map = y_part + (sin(theta) * x_obs) + (cos(theta) * y_obs);
// (6,3)
std::cout << int(round(x_map)) << ", " << int(round((y_map)) << std::endl;
return 0;
}
Observation 2 Solution
#include <cmath>
#include <iostream>
int main() {
// define coordinates and theta
double x_part, y_part, x_obs, y_obs, theta;
x_part = 4;
y_part = 5;
x_obs = 3;
y_obs = -2;
theta = -M_PI/2; // -90 degrees
// transform to map x coordinate
double x_map;
x_map = x_part + (cos(theta) * x_obs) - (sin(theta) * y_obs);
// transform to map y coordinate
double y_map;
y_map = y_part + (sin(theta) * x_obs) + (cos(theta) * y_obs);
// (2,2)
std::cout << int(round(x_map)) << ", " << int(round(y_map)) << std::endl;
return 0;
}
Observation 3 Solution
#include <cmath>
#include <iostream>
int main() {
// define coordinates and theta
double x_part, y_part, x_obs, y_obs, theta;
x_part = 4;
y_part = 5;
x_obs = 0;
y_obs = -4;
theta = -M_PI/2; // -90 degrees
// transform to map x coordinate
double x_map;
x_map = x_part + (cos(theta) * x_obs) - (sin(theta) * y_obs);
// transform to map y coordinate
double y_map;
y_map = y_part + (sin(theta) * x_obs) + (cos(theta) * y_obs);
// (0,5)
std::cout << int(round(x_map)) << ", " << int(round(y_map)) << std::endl;
return 0;
}